// Copyright 2008 Isis Innovation Limited
#include "System.h"
#include "OpenGL.h"
#include <gvars3/instances.h>
#include <stdlib.h>
#include "ATANCamera.h"
#include "MapMaker.h"
#include "Tracker.h"
#include "MapViewer.h"
#include <cvd/colourspace_convert.h>
#include <cvd/colourspaces.h>
#include <opencv/highgui.h>

using namespace CVD;
using namespace std;
using namespace GVars3;

System::System(bool bUseGUI)
{
  mbUseGUI = bUseGUI;

  // First, check if the camera is calibrated.
  // If not, we need to run the calibration widget.
  Vector<NUMTRACKERCAMPARAMETERS> vTest;

  vTest = GV3::get<Vector<NUMTRACKERCAMPARAMETERS> >("Camera.Parameters", ATANCamera::mvDefaultParams, HIDDEN);
  mpCamera = new ATANCamera("Camera");
  Vector<2> v2;
  if(v2==v2);
  if(vTest == ATANCamera::mvDefaultParams)
  {
    cout << endl;
    cout << "! Camera.Parameters is not set, need to run the CameraCalibrator tool" << endl;
    cout << "  and/or put the Camera.Parameters= line into the appropriate .cfg file." << endl;
    exit(1);
  }

  mirFrameSize = ImageRef(mpCamera->GetImageSize()[0], mpCamera->GetImageSize()[1]);
  mpIplImage = cvCreateImage(cvSize(mpCamera->GetImageSize()[0], mpCamera->GetImageSize()[1]), mpCamera->GetImageDepth(), mpCamera->GetImageChannels());
  mimFrameBW.resize(mirFrameSize);
  mimFrameRGB.resize(mirFrameSize);

  mpScarf = new Scarf(mirFrameSize);
  mpMapMaker = new MapMaker(*mpCamera, *mpScarf);
  mpTracker = new Tracker(*mpCamera, *mpScarf, *mpMapMaker);
  mpSharedMemClient = new PxSharedMemClient();

  if(mbUseGUI)
  {
    GUI.RegisterCommand("exit", GUICommandCallBack, this);
    GUI.RegisterCommand("quit", GUICommandCallBack, this);
    mGLWindow = new GLWindow2(mirFrameSize, "PTAM");
    mpMapViewer = new MapViewer(*mpCamera, *mGLWindow);
  
    GUI.ParseLine("GLWindow.AddMenu Menu Menu");
    GUI.ParseLine("Menu.ShowMenu Root");
    GUI.ParseLine("Menu.AddMenuButton Root Reset Reset Root");
    GUI.ParseLine("Menu.AddMenuButton Root \"Save Map\" SaveMap Root");
    GUI.ParseLine("Menu.AddMenuButton Root \"Pause Mapmaker\" TogglePause Root");

    GUI.ParseLine("DrawMap=0");
    GUI.ParseLine("Menu.AddMenuToggle Root \"View Map\" DrawMap Root");
  }
  
  mbDone = false;
  std::cout << "Fin creacion system" <<std::endl;
};


System::~System()
{
  cvReleaseImage(&mpIplImage);

  delete mpCamera;
  delete mpScarf;
  delete mpMapMaker;
  delete mpTracker;
  delete mpSharedMemClient;

  if(mbUseGUI)
  {
    delete mGLWindow;
    delete mpMapViewer;
  }
}


bool System::NewObservations(const mavlink_message_t* msg, SE3GaussianPDF &se3PdfGfromC)
{
  if(!mbDone)
  {
    if(!mpSharedMemClient->sharedMemCopyImage(msg, mpIplImage))
    {
      cout << "ERROR: Failed to read image from shared memory." << endl;
      return false;
    }
    else
    {
      if(mpCamera->GetImageChannels() == 3)
      {
        cvConvertImage(mpIplImage, mpIplImage, CV_CVTIMG_SWAP_RB);  //TODO: not necessary as not using colour frame, just saving it?
        BasicImage<CVD::Rgb<CVD::byte> > cim((CVD::Rgb<CVD::byte>*)(mpIplImage->imageData), mirFrameSize);
        mimFrameRGB.copy_from(cim);
        mimFrameBW = convert_image(mimFrameRGB);
      }
      else
      {
        // Copy from IplImage to CVD Image
        BasicImage<CVD::byte> bim((CVD::byte*)(mpIplImage->imageData), mirFrameSize);
        mimFrameBW.copy_from(bim);
        mimFrameRGB = convert_image(mimFrameBW);
      }
    }

    bool bTracking;
    uint64_t img_timestamp = mavlink_msg_image_available_get_timestamp(msg);
    std::vector<TooN::Vector<3> > vTest;

    if(mbUseGUI)
    {
      mGLWindow->SetupViewport();
      mGLWindow->SetupVideoOrtho();
      mGLWindow->SetupVideoRasterPosAndZoom();
      
      static gvar3<int> gvnDrawMap("DrawMap", 0, HIDDEN|SILENT);
      bool bDrawMap = *gvnDrawMap;// && mpMap->IsGood();

      bTracking = mpTracker->TrackFrame(img_timestamp, mimFrameBW, mimFrameRGB, !bDrawMap, se3PdfGfromC, vTest);
      
      if(bDrawMap)// && bTracking
        mpMapViewer->DrawMap(mpMapMaker->GetMap(), mpTracker->GetCamFromWorld(), vTest);

      string sCaption;
      if(bDrawMap && bTracking)
        sCaption = mpMapViewer->GetMessageForUser();
      else
        sCaption = mpTracker->GetMessageForUser();
      mGLWindow->DrawCaption(sCaption);
      mGLWindow->DrawMenus();
      mGLWindow->swap_buffers();
      mGLWindow->HandlePendingEvents();
    }
    else
      bTracking = mpTracker->TrackFrame(img_timestamp, mimFrameBW, mimFrameRGB, false, se3PdfGfromC, vTest);

    return false;
  }
  else
    return false;
}

void System::GUICommandCallBack(void *ptr, string sCommand, string sParams)
{
  if(sCommand=="quit" || sCommand == "exit")
    static_cast<System*>(ptr)->mbDone = true;
}

